#include "ActionUnit.h"
#include "../globals/globals.h"

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
后台执行线程的函数,通过事件广播来启动
*/
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ActionUnit::act_enc_routine_static_cb(void *p_this) { //静态函数
  ActionUnit *p_this_obj = (ActionUnit *)p_this;
  p_this_obj->data_act_enc.act_enc_running = true;

  static int id = 0;
  id++;

  DEBUG_COUT << endl
             << "***in routine  " << id << " :" << p_this_obj->unit_name << " "
             << p_this_obj->data_act_enc.aim_enc << "  "
             << " : start, aim=" << p_this_obj->data_act_enc.aim_enc << endl;
  /*调用系统函数转到指定位置,
    waitForCompletion为false,新的spin()或者spinFor()指令会取消上一个spinFor()和spinToPosition()指令
  */
 
  if (p_this_obj->data_act_enc.absolute_mode)
    p_this_obj->mtr_group.spinToPosition(
        p_this_obj->data_act_enc.aim_enc, p_this_obj->data_act_enc.units_r,
        p_this_obj->data_act_enc.speed, p_this_obj->data_act_enc.units_v,
        false); //绝对位置模式
  else
    p_this_obj->mtr_group.spinFor(
        p_this_obj->data_act_enc.aim_enc, p_this_obj->data_act_enc.units_r,
        p_this_obj->data_act_enc.speed, p_this_obj->data_act_enc.units_v,
        false); //相对位置模式

  wait(10); //切换cpu

  timer t;
  int n = 0;
  while (1) {
    //新增堵转退出
    if (t.time() > 100) {
      if (abs(p_this_obj->mtr_group.velocity(percentUnits::pct)) <= 1) //堵转
        n++;
      else
        n = 0;       //转动起来了
      if (n == 25) { //连续10次检测堵转
        p_this_obj->mtr_group.stop();
        cout << endl
             << "error:" << p_this_obj->unit_name << " motor "
             << " Stalled!!!!" << endl;
        ctrl.rumble(rumble_motor_stalled);
        break;
      }
    }
    //超时,提前停转
    if (t.time() > p_this_obj->data_act_enc.timeout &&
        p_this_obj->data_act_enc.timeout != 0) {
      DEBUG_COUT << "***in routine  " << id << " :" << p_this_obj->unit_name
                 << " " << p_this_obj->data_act_enc.aim_enc
                 << " break--------- timeout " << endl;
      if (!p_this_obj->data_act_enc.act_enc_force_stop)
        p_this_obj->mtr_group.stop();
      break;
    }

    //编码器数值接近，旋转位置到了,正常结束
    if (p_this_obj->mtr_group.isDone() &&
        p_this_obj->vec_ptr_motors[0]->installed()) {
      DEBUG_COUT << "***in routine  " << id << " :" << p_this_obj->unit_name
                 << " " << p_this_obj->data_act_enc.aim_enc
                 << " break--------- act enc OK " << endl;
      break;
    }

    //执行了stop_act_enc()函数, act_enc_force_stop被修改成true，提前终止
    if (p_this_obj->data_act_enc.act_enc_force_stop) {
      DEBUG_COUT << "***in routine  " << id << " :" << p_this_obj->unit_name
                 << " " << p_this_obj->data_act_enc.aim_enc
                 << " break--------- act_enc_force_stop" << endl;
      p_this_obj->mtr_group.stop();
      break;
    }
    wait(5);
  }

  if (p_this_obj->data_act_enc.delay_ending_time &&
      !p_this_obj->data_act_enc.act_enc_force_stop) {
    DEBUG_COUT << "***in routine  " << id << " :" << p_this_obj->unit_name
               << " " << p_this_obj->data_act_enc.aim_enc
               << " delay ending:" << p_this_obj->data_act_enc.delay_ending_time
               << "......" << endl;
    wait(p_this_obj->data_act_enc.delay_ending_time); //延迟结束
    DEBUG_COUT << "***in routine  " << id << " :" << p_this_obj->unit_name
               << " " << p_this_obj->data_act_enc.aim_enc
               << " delay ending done" << endl;
  }

  DEBUG_COUT << "***in routine  " << id << " :" << p_this_obj->unit_name << " "
             << p_this_obj->data_act_enc.aim_enc
             << " finally end ,  time :" << t.time() << " enc: "
             << p_this_obj->mtr_group.position(p_this_obj->data_act_enc.units_r)
             << endl;

  p_this_obj->data_act_enc.act_enc_force_stop = false;
  p_this_obj->data_act_enc.act_enc_running = false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
开启后台的管理函数
*/
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ActionUnit::_act_for_enc(bool absolute_mode, double aim_enc, double speed,
                              int timeout_ms, Block waitForCompletion,
                              bool wait_for_former_done, int delay_ending_time,
                              rotationUnits units_r, velocityUnits units_v) {
  if (timeout_ms < 0) {
    cout << unit_name << endl;
    cout << __PRETTY_FUNCTION__ << endl;
    cout << "error: timeout<=0\n\n";
    return;
  }

  int k = 0;
  for (auto iter : vec_ptr_motors) {
    if (!iter->installed()) {
      cout << endl << unit_name << " motor " << k << " Uninstalled!!!!" << endl;
    }
    k++;
  }

  if (data_act_enc.act_enc_running) {
    DEBUG_COUT << "   new _act_for_enc(),  aim=" << aim_enc;
    if (!(bool)wait_for_former_done) {
      stop_act_enc();
    } else {
      DEBUG_COUT << "  wait_for_former to finish" << endl;
      while (data_act_enc.act_enc_running)
        wait(5);
    }
  } else {
    DEBUG_COUT << "_______grand new _act_for_enc() begins...\n";
  }

  this->data_act_enc = {absolute_mode, aim_enc,    units_r,           speed,
                        units_v,       timeout_ms, delay_ending_time, false,
                        false};
  if (!(bool)waitForCompletion) { //不等待
    e_act_enc.broadcast();
    DEBUG_COUT << "_______wait for start...\n";
    while_wait(!data_act_enc.act_enc_running); //等待启动
    DEBUG_COUT << "_______background thread started!!!\n";
    // wait(10); // wait for start
  } else {
    e_act_enc.broadcastAndWait(); //等待执行结束
  }
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
关闭后台线程
*/
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ActionUnit::stop_act_enc() {
  // DEBUG_COUT << " of " << unit_name << " calls stop_act_enc()" << endl;
  if (data_act_enc.act_enc_running) {
    DEBUG_COUT << "    ----------------try to stop act enc\n";
    data_act_enc.act_enc_force_stop = true;
    while (data_act_enc.act_enc_running)
      wait(5);
    DEBUG_COUT << "    ----------------act_enc_routine() aim="
               << data_act_enc.aim_enc << " stopped\n";
  }
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
应用函数
*/
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ActionUnit::act_for_enc_absolute(double aim_enc, double speed,
                                      int timeout_ms, Block waitForCompletion,
                                      bool wait_for_former_done,
                                      int delay_ending_time,
                                      rotationUnits units_r,
                                      velocityUnits units_v) {
  _act_for_enc(true, aim_enc, speed, timeout_ms, waitForCompletion,
               wait_for_former_done, delay_ending_time, units_r, units_v);
}

void ActionUnit::act_for_enc_relative(double aim_enc, double speed,
                                      int timeout_ms, Block waitForCompletion,
                                      bool wait_for_former_done,
                                      int delay_ending_time,
                                      rotationUnits units_r,
                                      velocityUnits units_v) {
  _act_for_enc(false, aim_enc, speed, timeout_ms, waitForCompletion,
               wait_for_former_done, delay_ending_time, units_r, units_v);
}

bool ActionUnit::is_act_enc_running() { return data_act_enc.act_enc_running; }
